#ifndef __MAPPING_2D_H_
#define __MAPPING_2D_H_

#include <slam_2d_ros/common/slam_define.h>
#include <slam_2d_ros/common/frame.h>
#include <slam_2d_ros/mapping/submap.h>

#include <opencv2/core/core.hpp>
#include <opencv2/imgproc.hpp>

namespace slam_2d{

class Mapping2D {
    public:
        Mapping2D();
        ~Mapping2D();

        // 处理雷达数据
        bool processScan2D(Scan2DPtr scan);
        // 添加关键帧
        void AddKeyFrame();
        // 扩展地图
        void ExpandSubmap();

        // 可视化显示
        void Visualize2DScan(Scan2DPtr scan, const Pose2D& pose, cv::Mat& image, const Vec3b& color, int image_size = 800,
            float resolution = 20.0, const Pose2D& pose_submap = Eigen::Isometry2d::Identity());
        cv::Mat ShowGlobalMap(int max_size = 1000);

        // 获取轨迹数据
        vector<Pose2D> getTrajectory();
        // 获取当前位姿
        Pose2D getCurrentPose();
        // 获取地图数据
        cv::Mat getMap(Vec2f& origin);

    private:
        size_t frame_id_ = 0;       //帧ID
        size_t keyframe_id_ = 0;    //关键帧ID
        size_t submap_id_ = 0;      //子图ID
        bool first_scan_ = true;
        Pose2D motion_guess_ = Eigen::Isometry2d::Identity();
        boost::shared_ptr<Frame> current_frame_ = nullptr;
        boost::shared_ptr<Frame> last_frame_ = nullptr;
        boost::shared_ptr<Frame> last_keyframe_ = nullptr;

        boost::shared_ptr<Submap> current_submap_ = nullptr;
        std::vector<boost::shared_ptr<Submap>> all_submaps_;

        static constexpr double keyframe_pos_th_ = 0.3;              // 关键帧位移量
        static constexpr double keyframe_ang_th_ = 20 * M_PI / 180;  // 关键帧角度量
};

} //namespace slam_2d


#endif
